#pragma once

#include "i_base.h"

namespace idl
{
  /*SAD: the patch size is nxn*/
  template<typename COORDTYPE, unsigned int n/*patch width*/>
  class InterestPointSquaredSAD : public SquaredPatchInterestPointBase<unsigned char, COORDTYPE, n>
  {
  public:
    typedef SquaredPatchInterestPointBase<unsigned char, COORDTYPE, n> Base;
    using SquaredPatchInterestPointBase<unsigned char, COORDTYPE, n>::DATA_TYPE;
    using SquaredPatchInterestPointBase<unsigned char, COORDTYPE, n>::COORD_TYPE;
    using SquaredPatchInterestPointBase<unsigned char, COORDTYPE, n>::WIDTH;
    using SquaredPatchInterestPointBase<unsigned char, COORDTYPE, n>::DIM;
    using SquaredPatchInterestPointBase<unsigned char, COORDTYPE, n>::x;
    using SquaredPatchInterestPointBase<unsigned char, COORDTYPE, n>::y;
    using SquaredPatchInterestPointBase<unsigned char, COORDTYPE, n>::scale;
    using SquaredPatchInterestPointBase<unsigned char, COORDTYPE, n>::orientation;
    using SquaredPatchInterestPointBase<unsigned char, COORDTYPE, n>::descriptor;
   
    InterestPointSquaredSAD();
    ~InterestPointSquaredSAD(){};
    /*fast version, omit checking image boundary conditions -> user knows that the feature locations are safe*/
    int describe_s(const unsigned char *image, int width);
    /*safe version, check boundary conditions*/
    int describe(const unsigned char *image, int width, int height);
    int match(const InterestPointSquaredSAD<COORDTYPE, n>& tar, const int* param = NULL) const; /*match another interest point with SAD*/
  protected:
    unsigned int _rl, _rr;
  };

  template<typename COORDTYPE, unsigned int n>
  InterestPointSquaredSAD<COORDTYPE, n>::InterestPointSquaredSAD()
  {
    _rl = (n >> 1);
    _rr = n - _rl - 1;
  }

  template<typename COORDTYPE, unsigned int n>
  int InterestPointSquaredSAD<COORDTYPE, n>::describe_s(const unsigned char *image, int width)
  {
    int i;
    int frow = i_round(y) - (int)_rl; /*first    row*/
    int fcol = i_round(x) - (int)_rl; /*first column*/
    const unsigned char *pi = image + (frow *width) + fcol;
    unsigned char *d = descriptor;
    for (i = 0; i < n; ++i)
    {
      i_copy(pi, d, n);
      pi += width;
      d += n;
    }
    return 0;
  }

  template<typename COORDTYPE, unsigned int n>
  int InterestPointSquaredSAD<COORDTYPE, n>::describe(const unsigned char *image, int width, int height)
  {
    int ix = i_round(x);
    int iy = i_round(y);

    if (ix < 0 || iy < 0 || ix >= width || iy >= height)
    {
      return -1;
    }

    int i;
    int ymrl = iy - (int)_rl;
    int xmrl = ix - (int)_rl;

    int frow = i_max(0, ymrl);        /*first row*/
    int lrow = i_min(iy + (int)_rr, height - 1); /*last row*/
    int fcol = i_max(0, xmrl);        /*first column*/
    int lcol = i_min(ix + (int)_rr, width - 1); /*last column*/
    int nr_row = lrow - frow + 1;
    int nr_col = lcol - fcol + 1;

    if (nr_row == (int)n && nr_col == (int)n)
    {
      return describe_s(image, width);
    }
    
    int drow = frow - ymrl; /*frow must be >= ymrl*/
    int dcol = fcol - xmrl; /*fcol must be >= xmrl*/

    const unsigned char *pi = image + (frow *width) + fcol;
    unsigned char *d = descriptor + drow * n + dcol;
    
    for (i = frow; i <= lrow; ++i)
    {
      i_copy(pi, d, nr_col);
      pi += width;
      d += n;
    }

    return 0;
  }

  /*similarity score between two patches*/
  template<typename COORDTYPE, unsigned int n>
  int InterestPointSquaredSAD<COORDTYPE, n>::match(const InterestPointSquaredSAD<COORDTYPE, n>& tar, const int* param) const
  {
    int i, cost = 0;
    int trunc = (param == NULL) ? Constant<int>::MAX_VAL() : (*param);
    for (i = 0; i < DIM; ++i)
    {
      cost += i_min(trunc, i_abs((int)descriptor[i] - (int)(tar.descriptor[i])));
    }
    return cost;
  }

  /*ZSAD (zero mean): the patch size is nxn*/
  template<typename COORDTYPE, unsigned int n/*patch width*/>
  class InterestPointSquaredZSAD : public SquaredPatchInterestPointBase<short, COORDTYPE, n>
  {
  public:
    typedef SquaredPatchInterestPointBase<short, COORDTYPE, n> Base;
    using SquaredPatchInterestPointBase<short, COORDTYPE, n>::DATA_TYPE;
    using SquaredPatchInterestPointBase<short, COORDTYPE, n>::COORD_TYPE;
    using SquaredPatchInterestPointBase<short, COORDTYPE, n>::WIDTH;
    using SquaredPatchInterestPointBase<short, COORDTYPE, n>::DIM;
    using SquaredPatchInterestPointBase<short, COORDTYPE, n>::x;
    using SquaredPatchInterestPointBase<short, COORDTYPE, n>::y;
    using SquaredPatchInterestPointBase<short, COORDTYPE, n>::scale;
    using SquaredPatchInterestPointBase<short, COORDTYPE, n>::orientation;
    using SquaredPatchInterestPointBase<short, COORDTYPE, n>::descriptor;

    InterestPointSquaredZSAD();
    ~InterestPointSquaredZSAD(){};
    /*fast version, omit checking image boundary conditions -> user knows that the feature locations are safe*/
    int describe_s(const unsigned char *image, int width);
    /*safe version, check boundary conditions*/
    int describe(const unsigned char *image, int width, int height);
    int match(const InterestPointSquaredZSAD<COORDTYPE, n>& tar, const int* param = NULL) const; /*match another interest point with ZSAD*/
  protected:
    unsigned int _rl, _rr;
  };

  template<typename COORDTYPE, unsigned int n>
  InterestPointSquaredZSAD<COORDTYPE, n>::InterestPointSquaredZSAD()
  {
    _rl = (n >> 1);
    _rr = n - _rl - 1;
  }

  template<typename COORDTYPE, unsigned int n>
  int InterestPointSquaredZSAD<COORDTYPE, n>::describe_s(const unsigned char *image, int width)
  {
    int i; 
    unsigned int mean = 0;
    int frow = i_round(y) - (int)_rl; /*first    row*/
    int fcol = i_round(x) - (int)_rl; /*first column*/
    const unsigned char *pi = image + (frow * width) + fcol;
    short *d = descriptor;
    for (i = 0; i < n; ++i)
    {
      i_copy<unsigned char, short>(pi, d, n);
      pi += width;
      d += n;
    }
    for (i = 0; i < DIM; ++i)
    {
      mean += (unsigned int)descriptor[i];
    }
    mean /= DIM;
    for (i = 0; i < DIM; ++i)
    {
      descriptor[i] = (descriptor[i] - (short)mean); /*zero-mean*/
    }
    return 0;
  }

  template<typename COORDTYPE, unsigned int n>
  int InterestPointSquaredZSAD<COORDTYPE, n>::describe(const unsigned char *image, int width, int height)
  {
    int ix = i_round(x);
    int iy = i_round(y);

    if (ix < 0 || iy < 0 || ix >= width || iy >= height)
    {
      return -1;
    }

    int i, j;
    unsigned int mean = 0;
    int ymrl = iy - (int)_rl;
    int xmrl = ix - (int)_rl;

    int frow = i_max(0, ymrl);        /*first row*/
    int lrow = i_min(iy + (int)_rr, height - 1); /*last row*/
    int fcol = i_max(0, xmrl);        /*first column*/
    int lcol = i_min(ix + (int)_rr, width - 1); /*last column*/
    int nr_row = lrow - frow + 1;
    int nr_col = lcol - fcol + 1;

    if (nr_row == (int)n && nr_col == (int)n)
    {
      return describe_s(image, width);
    }

    int drow = frow - ymrl; /*frow must be >= ymrl*/
    int dcol = fcol - xmrl; /*fcol must be >= xmrl*/

    const unsigned char *pi = image + (frow * width) + fcol;
    short *dptr = descriptor + drow * n + dcol;
    short *d = dptr;

    for (i = frow; i <= lrow; ++i)
    {
      for (j = 0; j < nr_col; ++j)
      {
        d[j] = (short)pi[j];
        mean += (unsigned int)pi[j];
      }
      pi += width;
      d += n;
    }
    mean /= (nr_row * nr_col);

    d = dptr;
    for (i = frow; i <= lrow; ++i)
    {
      for (j = 0; j < nr_col; ++j)
      {
        d[j] = (d[j] - (short)mean); /*zero-mean*/
      }
      d += n;
    }
    return 0;
  }

  template<typename COORDTYPE, unsigned int n>
  int InterestPointSquaredZSAD<COORDTYPE, n>::match(const InterestPointSquaredZSAD<COORDTYPE, n>& tar, const int* param) const
  {
    int i, cost = 0;
    int trunc = (param == NULL) ? Constant<int>::MAX_VAL() : (*param);
    for (i = 0; i < DIM; ++i)
    {
      cost += i_min(trunc, i_abs((int)descriptor[i] - (int)(tar.descriptor[i])));
    }
    return cost;
  }

} /*namespace idl*/